package com.googlecode.curvedmetromaps.core.logic.costfunction;

import org.jbox2d.common.Vec2;

import com.googlecode.curvedmetromaps.core.geometry.BasicGeometry;
import com.googlecode.curvedmetromaps.core.geometry.BezierUtil;
import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroMap;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Cost function that keeps vertices apart.
 */
public class VertexEdgeDistanceCost extends BaseCost {

	public VertexEdgeDistanceCost(MetroMap map) {
		super(map);

		createWeight("base", "Base weight", 0.01);
		
		// Distance higher than the cutoff will not be considered.
		createWeight("cutoff", "Cut off distance", 0.5);

		// Distance lower than this value will be rounded up. 
		createWeight("mindist", "Minimum distance", 0.0001);
	}

	@Override
	public String getName() {
		return "Vertix-edge distance";
	}

	@Override
	public double calculateCost() {
		double result = 0;
		double cutoffSq = getWeight("cutoff");
		float minDistSq = (float) getWeight("mindist");

		for (MetroEdge e : map.edgeArray) {
			// Get Bézier control points.
			Vec2 v0 = e.getP0();
			Vec2 v1 = e.getP1();
			Vec2 cp = e.getCP();
			for (MetroStation m : map.stations) {
				int mid = m.id;
				Vec2 mpos = m.getPosition();
				// Skip stations that are part of this Bézier curve.
				if (e.src.id != mid && e.dst.id != mid) {
					// Get an approximation by calculating the distance to the
					// convex triangle of the Bézier curve.
					Vec2 approxProjection = BasicGeometry.pointTriangleProjection(mpos, v0, v1, cp);
					float approxDistSq = approxProjection.lengthSquared();
					if (approxDistSq < cutoffSq) {
						// Get Bézier projection
						Vec2[] controlPoints = { v0, cp, v1 };
						Vec2 projection = BezierUtil.pointProjection(mpos, controlPoints);
						float dist = projection.sub(mpos).lengthSquared();
						if (dist < minDistSq) dist = minDistSq;
						double cost = 1 / dist;
						result += cost;
					}
				}
			}
		}

		// Calculate average cost per vertex pair
		result /= map.stations.length * map.edgeArray.length;

		return getWeight("base") * result;
	}

}
